#include <cassert>
#include <boost/algorithm/string.hpp>
#include <map>

#include "../../../XMLTools.h"

#include "IMUSensor.h"

using namespace MMM;

IMUSensorPtr IMUSensor::loadSensorXML(RapidXMLReaderNodePtr node) {
    IMUSensorPtr sensor(new IMUSensor());
    sensor->loadSensor(node);
    return sensor;
}

IMUSensorPtr IMUSensor::loadSensorConfigurationXML(RapidXMLReaderNodePtr configurationNode) {
    IMUSensorPtr sensor(new IMUSensor());
    sensor->loadConfigurationXML(configurationNode);
    return sensor;
}

IMUSensor::IMUSensor() : AttachedSensor()
{
}

IMUSensor::IMUSensor(const std::string &segment, const std::string &offsetUnit, const Eigen::Matrix4f &offset, const std::string &description) :
    AttachedSensor(segment, offsetUnit, offset, description)
{
}

boost::shared_ptr<BasicSensor<IMUSensorMeasurement> > IMUSensor::cloneConfiguration() {
    IMUSensorPtr m(new IMUSensor(segment, offsetUnit, offset, description));
    return m;
}

void IMUSensor::loadMeasurementXML(RapidXMLReaderNodePtr node, float timestep, SensorMeasurementType type)
{
    assert(node);
    assert(node->name() == XML::NODE_MEASUREMENT);

    RapidXMLReaderNodePtr accNode = node->first_node("Acceleration");
    IMUSensorMeasurementPtr m(new IMUSensorMeasurement(timestep, XML::readFloatArray(accNode->value(), 3), type));
    addSensorMeasurement(m);
}

std::string IMUSensor::getType() {
    return TYPE;
}

std::string IMUSensor::getVersion() {
    return VERSION;
}

int IMUSensor::getPriority() {
    return 70;
}
